package autonomous;

import java.util.ArrayList;

import processing.core.PApplet;
import processing.core.PVector;

public class Boid extends PApplet {

	PVector loc;
	PVector vel;
	PVector acc;
	float r;
	float wandertheta;
	float maxforce; // Maximum steering force
	float maxspeed; // Maximum speed

	PApplet parent; //

	public Boid(PApplet p, PVector l, float ms, float mf) {
		parent = p; //
		acc = new PVector(0, 0);
		vel = new PVector(0, 0);
		loc = l.get();
		r = (float) 5.0;
		wandertheta = (float) 0.0;
		maxspeed = ms;
		maxforce = mf;
	}

	public void run() {
		update();
		borders();
		render();
	}

	// Method to update location
	void update() {
		// Update velocity
		vel.add(acc);
		// Limit speed
		vel.limit(maxspeed);
		loc.add(vel);
		// Reset accelertion to 0 each cycle
		acc.mult(0);
	}

	void seek(PVector target) {
		acc.add(steer(target, false));
	}

	void arrive(PVector target) {
		acc.add(steer(target, true));
	}

	public void wander() {
		float wanderR = 16.0f; // Radius for our "wander circle"
		float wanderD = 60.0f; // Distance for our "wander circle"
		float change = 0.25f;
		wandertheta += random(-change, change); // Randomly change wander theta

		// Now we have to calculate the new location to steer towards on the
		// wander circle
		PVector circleloc = vel.get(); // Start with velocity
		circleloc.normalize(); // Normalize to get heading
		circleloc.mult(wanderD); // Multiply by distance
		circleloc.add(loc); // Make it relative to boid's location

		PVector circleOffSet = new PVector(wanderR * cos(wandertheta), wanderR
				* sin(wandertheta));
		PVector target = PVector.add(circleloc, circleOffSet);
		acc.add(steer(target, false)); // Steer towards it

		// Render wandering circle, etc.
		// if (debug)
		drawWanderStuff(loc, circleloc, target, wanderR);

	}

	// A method that calculates a steering vector towards a target
	// Takes a second argument, if true, it slows down as it approaches the
	// target
	PVector steer(PVector target, boolean slowdown) {
		PVector steer; // The steering vector
		PVector desired = PVector.sub(target, loc); // A vector pointing from
													// the location to the
													// target
		float d = desired.mag(); // Distance from the target is the magnitude of
									// the vector
		// If the distance is greater than 0, calc steering (otherwise return
		// zero vector)
		if (d > 0) {
			// Normalize desired
			desired.normalize();
			// Two options for desired vector magnitude (1 -- based on distance,
			// 2 -- maxspeed)
			if ((slowdown) && (d < 100.0f))
				desired.mult(maxspeed * (d / 100.0f)); // This damping is
														// somewhat arbitrary
			else
				desired.mult(maxspeed);
			// Steering = Desired minus Velocity
			steer = PVector.sub(desired, vel);
			steer.limit(maxforce); // Limit to maximum steering force
		} else {
			steer = new PVector(0, 0);
		}
		return steer;
	}

	public void follow(Path p) {

		// Predict location 25 (arbitrary choice) frames ahead
		PVector predict = vel.get();
		predict.normalize();
		predict.mult(25);
		PVector predictLoc = PVector.add(loc, predict);

		// Draw the predicted location
		if (true) {
			// if (debug) {
			parent.fill(0);
			parent.stroke(0);
			parent.line(loc.x, loc.y, predictLoc.x, predictLoc.y);
			parent.ellipse(predictLoc.x, predictLoc.y, 4, 4);
		}

		// Now we must find the normal to the path from the predicted location
		// We look at the normal for each line segment and pick out the closest
		// one
		PVector target = null;
		PVector dir = null;
		float record = 1000000; // Start with a very high record distance that
								// can easily be beaten

		// Loop through all points of the path
		for (int i = 0; i < p.points.size() - 1; i++) {

			// Look at a line segment
			PVector a = (PVector) p.points.get(i);
			PVector b = (PVector) p.points.get(i + 1);

			// Get the normal point to that line
			PVector normal = getNormalPoint(predictLoc, a, b);

			// Check if normal is on line segment
			float da = PVector.dist(normal, a);
			float db = PVector.dist(normal, b);
			PVector line = PVector.sub(b, a);
			// If it's not within the line segment, consider the normal to just
			// be the end of the line segment (point b)
			if (da + db > line.mag() + 1) {
				normal = b.get();
			}

			// How far away are we from the path?
			float d = PVector.dist(predictLoc, normal);
			// Did we beat the record and find the closest line segment?
			if (d < record) {
				record = d;
				// If so the target we want to steer towards is the normal
				target = normal;

				// Look at the direction of the line segment so we can seek a
				// little bit ahead of the normal
				dir = line;
				dir.normalize();
				// This is an oversimplification
				// Should be based on distance to path & velocity
				dir.mult(10);
			}
		}

		// Draw the debugging stuff
		// if (debug) {
		if (true) {

			// Draw normal location
			parent.fill(0);
			parent.noStroke();
			parent.line(predictLoc.x, predictLoc.y, target.x, target.y);
			parent.ellipse(target.x, target.y, 4, 4);
			parent.stroke(0);
			// Draw actual target (red if steering towards it)
			parent.line(predictLoc.x, predictLoc.y, target.x, target.y);
			if (record > p.radius)
				parent.fill(255, 0, 0);
			parent.noStroke();
			parent.ellipse(target.x + dir.x, target.y + dir.y, 8, 8);
		}

		// Only if the distance is greater than the path's radius do we bother
		// to steer
		if (record > p.radius) {
			target.add(dir);
			seek(target);
		}
	}

	// A function to get the normal point from a point (p) to a line segment
	// (a-b)
	// This function could be optimized to make fewer new Vector objects
	PVector getNormalPoint(PVector p, PVector a, PVector b) {
		// Vector from a to p
		PVector ap = PVector.sub(p, a);
		// Vector from a to b
		PVector ab = PVector.sub(b, a);
		ab.normalize(); // Normalize the line
		// Project vector "diff" onto line by using the dot product
		ab.mult(ap.dot(ab));
		PVector normalPoint = PVector.add(a, ab);
		return normalPoint;
	}

	void render() {
		// Draw a triangle rotated in the direction of velocity
		float theta = vel.heading2D() + radians(90);
		parent.fill(175);
		parent.stroke(0);
		parent.pushMatrix();
		parent.translate(loc.x, loc.y);
		parent.rotate(theta);
		parent.beginShape(TRIANGLES);
		parent.vertex(10, -r * 12);
		parent.vertex(-r, r * 12);
		parent.vertex(r, r * 12);
		parent.endShape();
		parent.popMatrix();
	}

	// Wraparound
	void borders() {
		if (loc.x < -r)
			loc.x = parent.width + r;
		if (loc.y < -r)
			loc.y = parent.height + r;
		if (loc.x > parent.width + r)
			loc.x = -r;
		if (loc.y > parent.height + r)
			loc.y = -r;
	}

	// A method just to draw the circle associated with wandering
	void drawWanderStuff(PVector loc, PVector circle, PVector target, float rad) {
		parent.stroke(0);
		parent.noFill();
		parent.ellipseMode(CENTER);
		parent.ellipse(circle.x, circle.y, rad * 2, rad * 2);
		parent.ellipse(target.x, target.y, 4, 4);
		parent.line(loc.x, loc.y, circle.x, circle.y);
		parent.line(circle.x, circle.y, target.x, target.y);
	}

}